#include "elisa/elisa.h"
#include <iostream>
#include <string>

#include "Modeler.h"

using namespace std;
using namespace elisa;

template<typename Var> 
void FallingInvCont(int n, Var *dv, const Var *v, Var t, void *param)
{
	static const interval g(9.8);
	static const interval k(0.3);

	// x
	dv[0] = v[1];
	dv[1] = interval::zero;
	// y
	dv[2] = v[3];
	dv[3] = -g + k*v[3];
}

class FallingInv : public Modeler
{
private:
	static const interval e;
	static const int n;

	Time t;
	ContVar x;
	ContVar y;
	ContVarVec cvv;

	static interval nextT;
	static interval nextX, nextVX;
	static interval nextY, nextVY;

	ADPtr ad;
	elisa::RealConstraint ac;
	ContConstraint m_cc;

public:
	FallingInv()
		: ad(new vnodelp::FADBAD_AD(n, Falling1Cont, Falling1Cont)),
		  x(2, "x"),
		  y(2, "y"),
		  t(width(nextT), sup(nextT) + width(nextT)*2),
		  //t(width(nextT), inf(nextT) - width(nextT)*2),
		  m_cc(new ContConstraintRep(ad, cvv, t)),

		  ac(y == sin(x))
	{
		double p((nextT.width() + nextX.width() + nextY.width()) / 3);
		if (p != 0.0)
			elisa::RealVar::SetPrecision(p);
		//elisa::RealVar::SetPrecision(1.0e-9);
		p = 1.0e-9;
		t.setPrecision(p > width(nextT)*2 ? p : width(nextT)*2);

		x.dot(0, nextX);
		x.dot(1, nextVX);
		y.dot(0, nextY);
		y.dot(1, nextVY);
		cvv.push_back(&x);
		cvv.push_back(&y);

		t.setDomain(v_bias::interval(inf(nextT), 10.0), cvv);
		//t.setDomain(v_bias::interval(-0.1, sup(nextT)), cvv);
#ifdef _DEBUG
cout << "t0: " << nextT << ", y0: " << nextY << ", vy0: " << nextVY << endl;
#endif
		nextT = interval::emptyset;
	}

	elisa::Model *model()
	{
		return new Model(m_cc && ac);
	}

	elisa::ContConstraint & cc() 
	{
		return m_cc;
	}

	void reset()
	{
		interval nt = t.domain().getTime();
		const iVector & ae = t.domain().getAE();
		interval nx = interval(x.domain().inf(), x.domain().sup());
		interval ny = interval(y.domain().inf(), y.domain().sup());

#ifdef _DEBUG
		interval nvx = ((interval(1) - v_bias::pow(e*v_bias::cos(nx), 2)) * ae[1] + (interval(1)+e) * v_bias::cos(nx) * ae[3]) 
			/ (interval(1) + v_bias::pow(v_bias::cos(nx), 2));
		interval nvy = ((interval(1)+e) * v_bias::cos(nx) * ae[1] + (-e + v_bias::pow(v_bias::cos(nx), 2)) * ae[3]) 
			/ (interval(1) + v_bias::pow(v_bias::cos(nx), 2));
#endif

		if (nextT.is_empty()) {
			nextT = nt;
			nextX = nx;
			nextY = ny;
#ifdef _DEBUG
			nextVX = nvx;
			nextVY = nvy;
#endif
		} else {
			nextT  = hull(nextT, nt);
			nextX  = hull(nextX, nx);
			nextY  = hull(nextY, ny);
#ifdef _DEBUG
			nextVX = hull(nextVX, nvx);
			nextVY = hull(nextVY, nvy);
#endif
		}
	}
	
	void print()
	{
		const iVector & ae = t.domain().getAE();
		cout << '\t' << "x " << " = " << ae[0] << endl;
		cout << '\t' << "x'" << " = " << ae[1] << endl;
		cout << '\t' << "y " << " = " << ae[2] << endl;
		cout << '\t' << "y'" << " = " << ae[3] << endl;
	}

	/*
	RealVar & varX()
	{
		return t.domain().getTime();
	}
	RealVar & varY()
	{
		return y;
	}
	*/
};

//const interval FallingInv::e(0.8);
const interval FallingInv::e(1.25);
const int FallingInv::n(4);

interval FallingInv::nextT(interval::zero);
//interval FallingInv::nextT(2.6849403499049371, 2.6849403653713422);
interval FallingInv::nextX(12.648187410559268, 12.648190024566942);
interval FallingInv::nextVX(4.2685295379357822, 4.2685307248333215);
interval FallingInv::nextY(0.081723267215066939, 0.081725827556840824);
interval FallingInv::nextVY(-6.0256903646024718, -6.0256876921705684);
